#include "stdafx.h"
#include "kd.h"
#include "../impl/lwobject.h"

//max depth for the tree
#define MAX_DEPTH 1000
//sah true or false
#define OUT 0
#define SAH 1

;namespace kd_build_internal
{
	struct BuildStateStruct
	{
		size_t nodeIndex;
		BBox scene;
		std::stack<size_t> objects;
		
		int depth;
	};

	struct stackInfo{
		size_t id;
		float distance ;
		bool left; //1 for left 0 for right
		bool camleft;
	};
}

using namespace kd_build_internal;

//An iterative split in the middle build for KDs
void KD::build(const std::vector<Primitive*> &_objects)
{
	//we need a copy of _objects for primitive splitting
	//need one BBox around the complete scene
	 
	//split in the middle of the longest edge of the actual group
	//to find out if a Primitive is in the left or right child, compare the splitPos with the Centroid of the BBox of the Primitive
	//put the children on the stack and handle the next object on the stack
	//stop if stack is empty

	//vector with all BBoxes of all Primitives
	std::vector<BBox> objectBBoxes(_objects.size());

	//struct with the BBox of the whole scene and the build stack
	BuildStateStruct curState;

	//initialize the depth of the tree
	curState.depth = 0;

	curState.scene = BBox::empty();
	scene = BBox::empty();
	std::cout<<"kdbbox"<<std::endl;
	//fill the objectBBoxes-vector and create the BBox around the whole scene
	for(size_t i=0;i<objectBBoxes.size();i++)
	{
		//save BBox for every object in the scene to compute the position of the object in the scene
		objectBBoxes[i] = _objects[i]->getBBox();
		//put the index of this object on the actual state's stack
		curState.objects.push(i);
		//extend the scene BBox by every object's BBox
		curState.scene.extend(objectBBoxes[i]);
	}
	std::cout<<"kdbboxend"<<std::endl;
	//save the BBox of the whole scene in the KD-tree
	scene = curState.scene;
	//std::cout<<scene.min[1]<<"-"<<scene.max[1]<<std::endl;
	
	/*std::cout<<"boxmin: "<<curState.scene.min[0]<<"|"<<curState.scene.min[1]<<"|"<<curState.scene.min[2]<<std::endl;
	std::cout<<"boxmax: "<<curState.scene.max[0]<<"|"<<curState.scene.max[1]<<"|"<<curState.scene.max[2]<<std::endl;
	std::cout<<"boxmin: "<<scene.min[0]<<"|"<<scene.min[1]<<"|"<<scene.min[2]<<std::endl;
	std::cout<<"boxmax: "<<scene.max[0]<<"|"<<scene.max[1]<<"|"<<scene.max[2]<<std::endl;*/

	//set the index of the first node
	curState.nodeIndex = 0;
	//prepare the kd-tree by setting the m_nodes size to 1
	m_nodes.resize(1);
	
	//build the stack for the pseudo recursion
	std::stack<BuildStateStruct> buildStack;

	const float _EPS = 0.0000001f;

	size_t leafs = 0,
		maxchild = 0,
		minchild = INT_MAX,
		allchild = 0,
		beginobjcnt = curState.objects.size();

	//flag if sah founds a splitvalue, if not make child
	bool foundcost=true;

	int maxdepth = 0;

	//the real build procedure, breaked, if stack gets empty
	for(;;)
	{
		//std::cout<<curState.objects.size()<<std::endl;
		if(OUT==1)
		{
			std::cout<<std::endl;
			std::cout<<"BuildStack: "<<buildStack.size()<<std::endl;
		}
		//diagonal of the sceneBBox
		Vector diag = curState.scene.diagonal();

		//the dimension of the longest edge
		int longEdge = (diag.x>diag.y) ? ((diag.x>diag.z) ? 0 : 2) : ((diag.y>diag.z) ? 1 : 2);
		if(OUT==1) std::cout<<"TreeDepth: "<<curState.depth<<std::endl;

		//if actual part contains to less objects, is to small or a specific depth is reached, then create a leaf
		if(curState.objects.size() < 3 || fabs(diag[longEdge])<_EPS || curState.depth>=MAX_DEPTH || !foundcost)
		{
			//std::cout<<"childs: "<<curState.objects.size()<<std::endl;

			foundcost=true;

			maxdepth = std::max(maxdepth, curState.depth);

			//save where the children of the actual node are saved in the leafData-vector
			const size_t NODE_TYPE_MASK = ((size_t)1 << Node::LEAF_FLAG_BIT);
			m_nodes[curState.nodeIndex].dataIndex = m_leafData.size() | NODE_TYPE_MASK;

			leafs += 1;
			//primitives = primitives + curState.objects.size();
			maxchild = maxchild<curState.objects.size() ? curState.objects.size() : maxchild;
			minchild = std::min(minchild, curState.objects.size());
			allchild += curState.objects.size();

			
			//save the children
			while(!curState.objects.empty())
			{
				m_leafData.push_back(_objects[curState.objects.top()]);
				curState.objects.pop();
			}
			//put behind the children a NULL-child, to group the children of every node
			m_leafData.push_back(NULL);

			//if all objects are saved in the tree
			if(buildStack.empty())
				break;

			//else take the next unfinished part from the stack
			curState = buildStack.top();
			buildStack.pop();

			continue;
		}
		
		//calculate and save the position of the split-plane
		float splitVal = (curState.scene.min[longEdge] + curState.scene.max[longEdge]) / 2.f;
		
		std::stack<size_t> bestleftStack;
		std::stack<size_t> bestrightStack;
		float splitplanecount=0;

		
	
		if(SAH==1){
			foundcost=false;
			//calculate sah split-plane
			float height[3] = {curState.scene.max[2]-curState.scene.min[2],curState.scene.max[0]-curState.scene.min[0],curState.scene.max[1]-curState.scene.min[1]};
			float depth[3] = {curState.scene.max[1]-curState.scene.min[1],curState.scene.max[2]-curState.scene.min[2],curState.scene.max[0]-curState.scene.min[0]};
			float bestcost = FLT_MAX;
			float bestpos = 0;
			int bestDim = 0,
				bestPrim = curState.objects.size();

			//work on a copy of the actual State stack
			std::stack<size_t> prim (curState.objects);
			if(OUT==1) std::cout<<"PrimStacksize: "<<prim.size()<<std::endl;
			float lastWidth[3]={0,0,0};
			float width[3], width2[3], split[3], gap[3];
			for(int i=0;i<3;i++){
				gap[i]= (curState.scene.max[i]-curState.scene.min[i])/100.f;
				width[i]= curState.scene.min[i]-gap[i];
			
			}

			//test the splitVal for every raster position
			while((curState.scene.min[1]+width[1])< curState.scene.max[1]){
				for(int i=0;i<3;i++){
					width[i]+= gap[i];
				}
				
				//compute the width of both parts
				for(int i=0;i<3;i++)
				{
					width2[i] = curState.scene.max[i]-curState.scene.min[i]-width[i];
					split[i] = curState.scene.min[i] + width[i];
				}
			/*	//if the distance to previous computed splitVal is too small, skip this one
				if(curState.objects.size()>13337){
					if(bestcost!=FLT_MAX
						&& (fabs(split[0]-lastWidth[0])<(curState.scene.diagonal()[0]/10.f)
							|| fabs(split[1]-lastWidth[1])<(curState.scene.diagonal()[1]/10.f)
							|| fabs(split[2]-lastWidth[2])<(curState.scene.diagonal()[2]/10.f)))
						continue;
				}
				else
					if(bestcost!=FLT_MAX
						&& (fabs(split[0]-lastWidth[0])<(curState.scene.diagonal()[0]/10.f)
							&& fabs(split[1]-lastWidth[1])<(curState.scene.diagonal()[1]/10.f)
							&& fabs(split[2]-lastWidth[2])<(curState.scene.diagonal()[2]/10.f)))
						continue;

				//if split is too near to the last split, then skip in this dimension
				if(bestcost!=FLT_MAX)
					for(int i=0;i<3;i++)
						if(fabs(split[i]-lastWidth[i])<(curState.scene.diagonal()[i]/10.f))
							width[i] = -1;*/

				//calculate the areas under the parts
				float area[3], area2[3],totarea[3];
				for(int i=0;i<3;i++){
					area[i] = 2*(width[i]*height[i]+depth[i]*height[i]+width[i]*depth[i]);
					area2[i] = 2*(width2[i]*height[i]+depth[i]*height[i]+width2[i]*depth[i]);
					totarea[i]= 2*((width2[i]+width[i])*height[i]+depth[i]*height[i]+(width2[i]+width[i])*depth[i]);
				}

				//create a copy of the actual State stack to extract how many objects are on the right/left side
				std::stack<size_t> countprim (curState.objects);

				std::stack<size_t> leftStack[3];
				std::stack<size_t> rightStack[3];

				//split the Primitives in two the left and right part
				while(!countprim.empty()){
					size_t curcountPrim = countprim.top();
					countprim.pop();

					//left-right-split Dim 0..2
					for(int i=0;i<3;i++){
						if(objectBBoxes[curcountPrim].min[i] >= split[i])
							rightStack[i].push(curcountPrim);
						else 
							if(objectBBoxes[curcountPrim].max[i] <= split[i])
								leftStack[i].push(curcountPrim);
							else{
									rightStack[i].push(curcountPrim);
									leftStack[i].push(curcountPrim);
									splitplanecount++;
							}
					}
				}

				//count the number of primitives in both stacks
				size_t leftcount[3], rightcount[3];
				
					for(int i=0;i<3;i++){
						leftcount[i] = leftStack[i].size();
						rightcount[i] = rightStack[i].size();
					}
					
				//the real costs of this splitVal
				float cost[3];
				for(int i=0;i<3;i++)
				{
					if(leftcount[i]!=0 && rightcount[i]!=0)
						cost[i] = (float)(/*splitplanecount*/((area[i]/totarea[i])*(leftcount[i])+(area2[i]/totarea[i])*(rightcount[i])));
					else
						cost[i] = FLT_MAX;
				}
				
				//if the actual costs are better, save them
				for(int i=0;i<3;i++){
					if( curState.objects.size()!=leftcount[i] && curState.objects.size()!=rightcount[i]				//child-nodes contain other #objects as actual node												//if object-size small, you can split wherever you want
																														//if objects-size bigger, the child-nodes must have min-ratio
						)
							if(width[i]>0 && width2[i]>0)
							if(cost[i]<bestcost){
								bestcost = cost[i];
								bestpos = split[i];
								bestDim = i;
								bestleftStack = leftStack[i];
								bestrightStack = rightStack[i];
								foundcost=true;

								lastWidth[i] = split[i];
							}
				}
			}
				
			if(OUT==1) 
			{
				std::cout<<"leftcount: "<<bestleftStack.size()<<" rightcount: "<<bestrightStack.size()<<std::endl;
				std::cout<<"bestPos: "<<bestpos<<" foundcost: "<<foundcost<<std::endl;
			}

			if(!foundcost) continue;
			longEdge=bestDim;
			splitVal=bestpos;
		}
		//split without SAH
		else
		{
			std::stack<size_t> countprim(curState.objects);
			while(!countprim.empty()){
						size_t curcountPrim = countprim.top();
						countprim.pop();

						if(objectBBoxes[curcountPrim].min[longEdge] >= splitVal)//(objectBBoxes[curcountPrim].min[i]-curState.scene.min[i])>=width[i])
							bestrightStack.push(curcountPrim);
						else 
							if(objectBBoxes[curcountPrim].max[longEdge] <= splitVal)//objectBBoxes[curcountPrim].max[i]-curState.scene.min[i]<width[i])
								bestleftStack.push(curcountPrim);
							else
							{
								bestrightStack.push(curcountPrim);
								bestleftStack.push(curcountPrim);
							}
					}
		}
		if(OUT==1) std::cout<<" Bestpos: "<<splitVal<<" LongEdge: "<<longEdge<<" SceneMin: "<<scene.min[longEdge]<<" SceneMax: "<<scene.max[longEdge]<<std::endl;

		//save where you can find the child node of the actual node
		m_nodes[curState.nodeIndex].dataIndex = m_nodes.size();

		//save the dimension and the coordinate in which the split occured
		m_nodes[curState.nodeIndex].dimension = longEdge;
		m_nodes[curState.nodeIndex].splitPos = splitVal;
		
		//create the state for the right part and update the ones for the left part
		curState.nodeIndex = m_nodes.size();
		curState.scene = BBox::empty();
		curState.objects = bestleftStack;
		curState.depth = curState.depth+1;

		BuildStateStruct rightState;
		rightState.nodeIndex = curState.nodeIndex + 1;
		rightState.scene = BBox::empty();
		rightState.objects = bestrightStack;
		rightState.depth = curState.depth;

		//initialize the BBoxes for the right and the left part
		std::stack<size_t> helpleft(bestleftStack),
							helpright(bestrightStack);
		while(!helpleft.empty())
		{
			size_t actual = helpleft.top();
			helpleft.pop();

			curState.scene.extend(objectBBoxes[actual].min+objectBBoxes[actual].diagonal()/2.f);
		}
		while(!helpright.empty())
		{
			size_t actual = helpright.top();
			helpright.pop();

			rightState.scene.extend(objectBBoxes[actual].min+objectBBoxes[actual].diagonal()/2.f);
		}

		//put the right part on the buildstack and continue with the left part
		buildStack.push(rightState);

		//extend the m_nodes vector for the next nodes
		m_nodes.resize(rightState.nodeIndex + 1);
	}

	std::cout<<"\n#leafs: "<<leafs<<"\nmaxchild: "<<maxchild<<"\nminchild: "<<minchild<<std::endl;
	std::cout<<"\ntotal #childs: "<<allchild<<"\naverage child: "<<allchild/beginobjcnt<<std::endl;
	std::cout<<"max Depth: "<<maxdepth<<std::endl;
	if(SAH==1)
			std::cout<<"Build with KD with SAH"<<std::endl;
	else
		std::cout<<"Build with KD without SAH "<<std::endl;
}


//Recursive intersection
KD::IntersectionReturn KD::intersect(const Ray &_ray, float _previousBestDistance) const
{
	
	//if ray hits splitting-plane, we have to take care of both children
	//else we have to compare the coordinate of the ray's sourcepoint Cr with the split-coordinate of the actual node Cs:
	//		1) if Cr<Cs then take care of left child
	//		2) if Cr>Cs then take care of the right child

	Primitive::IntRet bestHit;
	bestHit.distance = _previousBestDistance;

	Primitive *bestPrimitive = NULL;
	//if(x==30 && y==200) std::cout<<"scene.min "<<scene.min.x<<"|"<<scene.min.y<<"|"<<scene.min.z<<" scene.max "<<scene.max.x<<"|"<<scene.max.y<<"|"<<scene.max.z<<"\n\n"<<std::endl;
	//intersection-distances with the scene BBox
	std::pair<float,float> pair = scene.intersect(_ray);
	float t_near = pair.first,
		t_far = pair.second;
	
	//boolean for breaking if a intersection was found
	bool hitten = false;
	
	const float _EPS = 0.0000001f;

	//stack with the elements which must get handled
	std::stack<stackInfo> traverseStack;

	stackInfo curNode;
	curNode.id= 0;
	curNode.distance=FLT_MAX;
	curNode.left=1;

	//float lastDistance=FLT_MAX;

	for(;;)
	{
		const KD::Node& node = m_nodes[curNode.id];

		/**	this part is taken from the BVH-method
			should be the same strategy	**/
		if(node.isLeaf())
		{
			//take the first child of the leaf
			size_t idx = node.getLeftChildOrLeaf();
			//while loop over all children of the actual node
			int childs = 0;
			while(m_leafData[idx] != NULL)
			{
				//intersect the actual child with the ray
				Primitive::IntRet curRet = m_leafData[idx]->intersect(_ray, bestHit.distance);
				//if an intersection occurs and the distance to this intersection is shorter than the actual best
				if(curRet.distance > Primitive::INTEPS() && curRet.distance < bestHit.distance)
				{
					//save the better distance and hitten object
					bestHit = curRet;
					bestPrimitive = m_leafData[idx];

					if(curNode.left){
						if(curNode.camleft){
							if(bestHit.distance<=curNode.distance)
								if(curNode.distance>t_far)
									hitten = true;
						}
						else{
							if(bestHit.distance>=curNode.distance)
								if(curNode.distance>t_far)
									hitten = true;
						}
					}
					else{
						if(curNode.camleft){
							if(bestHit.distance>=curNode.distance)
								if(curNode.distance<t_near)
									hitten = true;
						}
						else{
							if(bestHit.distance<=curNode.distance)
								if(curNode.distance<t_near)
									hitten = true;
						}
					}
				}
				//increase counter for the childindex
				idx++;
				childs++;
			}
			//if a hitpoint was found or all parts of the tree were handled
			//if(x==30 && y==200) std::cout<<"#childs "<<childs<<" hitten "<<hitten<<std::endl;
			if(hitten || traverseStack.empty())
				break;
			//else take the next part from the stack
			curNode = traverseStack.top();
			traverseStack.pop();
		}
		/**	end of the copied part	**/

		//if the actual part is an inner node
		else
		{
			//compute the distance between the origin of the ray and the hitpoint with the splitPlane
			float distance = (node.splitPos-_ray.o[node.dimension])*(1.f/_ray.d[node.dimension]);
			
			float dis = distance;

			Vector d = (node.dimension==0)?Vector(1,0,0):((node.dimension==1)?Vector(0,1,0):Vector(0,0,1));
			Point o = (node.dimension==0)?Point(node.splitPos,0,0):((node.dimension==1)?Point(0,node.splitPos,0):Point(0,0,node.splitPos));

			Plane plane(o,d);

			distance = plane.intersect(_ray);
			/*if(x==30 && y==200){
				Point p = _ray.o + _ray.d*distance;
				std::cout<<"\nplaneintersect-point "<<p.x<<"|"<<p.y<<"|"<<p.z<<std::endl;
				std::cout<<"t_far "<<t_far<<" t_near "<<t_near<<" distance "<<distance<<std::endl;
			}*/
			
			stackInfo rightNode;
			//if cam is on the left side of the splitplane
			if(_ray.o[node.dimension] <= node.splitPos){
				curNode.camleft = 1;
				rightNode.camleft = 1;
			}
			//if the cam is on the right side of the splitplane
			else{
				curNode.camleft = 0;
				rightNode.camleft = 0;
			}

			//if ray's origin lays on the splitplane
			if(distance==0)
			{
				float onRay = _ray.o[node.dimension] + _ray.d[node.dimension];
				//if whole ray lays on the splitplane
				if(onRay == node.splitPos)
				{
					curNode.id = node.getLeftChildOrLeaf();
					curNode.distance=distance;
					curNode.left=1;
					rightNode.id=node.getLeftChildOrLeaf()+1;
					rightNode.distance=distance;
					rightNode.left=0;
					traverseStack.push(rightNode);
					continue;
				}
				//if ray leaves splitplane to the left side
				if(onRay < node.splitPos){
					curNode.id = node.getLeftChildOrLeaf();
					curNode.distance=distance;
					curNode.left=1;

				}
				//if ray leaves splitplane to the right side
				else{
					curNode.id = node.getLeftChildOrLeaf()+1;
					curNode.distance=distance;
					curNode.left=0;
				}
				continue;
			}

			//if the splitPlane isn't hitten by the ray
			if(distance<0)
			{
				//if the origin of the ray is on the left side of the plane
				if(_ray.o[node.dimension] < node.splitPos){
					//continue with the left part
					curNode.id = node.getLeftChildOrLeaf();
					curNode.distance=distance;
					curNode.left=1;
					//if(x==30 && y==200) std::cout<<"dist<0, go left"<<std::endl;
				}
				//if the origin of the ray is on the right side of the plane
				else{
					//continue with the right part
					curNode.id = node.getLeftChildOrLeaf()+1;
					curNode.distance=distance;
					curNode.left=0;
					//if(x==30 && y==200) std::cout<<"dist<0, go right"<<std::endl;
				}
				continue;
			}

			//if the splitPlane is hitten by the ray
			//if ray hit the splitplane before entering the sceneBBox
			if(t_near-distance > 0)
			{
				if(_ray.o[node.dimension] < node.splitPos){
					curNode.id = node.getLeftChildOrLeaf()+1;
					curNode.distance=distance;
					curNode.left=0;
					//if(x==30 && y==200) std::cout<<"hit befor t_near, go right"<<std::endl;
				}
				else{
					curNode.id = node.getLeftChildOrLeaf();
					curNode.distance=distance;
					curNode.left=1;
					//if(x==30 && y==200) std::cout<<"hit befor t_near, go left"<<std::endl;
				}
				continue;
			}
			//if ray hit the splitplane after leaving the sceneBBox
			if(distance-t_far > 0)
			{
				if(_ray.o[node.dimension] < node.splitPos){
					curNode.id = node.getLeftChildOrLeaf();
					curNode.distance=distance;
					curNode.left=1;
					//if(x==30 && y==200) std::cout<<"hit behind t_far, go left"<<std::endl;
				}
				else{
					curNode.id = node.getLeftChildOrLeaf()+1;
					curNode.distance=distance;
					curNode.left=0;
					//if(x==30 && y==200) std::cout<<"hit behind t_far, go right"<<std::endl;
				}
				continue;
			}

			//if ray hits the splitplane between t_near and t_far
			if(_ray.o[node.dimension] < node.splitPos)
			{
				//continue with the left part and put the right part on the stack
				curNode.id = node.getLeftChildOrLeaf();
					curNode.distance=distance;
					curNode.left=1;
					stackInfo rightNode;
					rightNode.id=node.getLeftChildOrLeaf()+1;
					rightNode.distance=distance;
					rightNode.left=0;
					traverseStack.push(rightNode);
					//if(x==30 && y==200) std::cout<<"ray.o is left, go left then right"<<std::endl;
			}
			//if ray travels through the right part first
			else
			{
				//continue with the right part and put the left part on the stack
				curNode.id = node.getLeftChildOrLeaf()+1;
					curNode.distance=distance;
					curNode.left=0;
					stackInfo leftNode;
					leftNode.id=node.getLeftChildOrLeaf();
					leftNode.distance=distance;
					leftNode.left=1;
					traverseStack.push(leftNode);
					//if(x==30 && y==200) std::cout<<"ray.o is right, go right then left"<<std::endl;
			}
		}
	}
	//create the return struct and save the distance and the hitten primitive in it
	BVH::IntersectionReturn ret;
	ret.ret = bestHit;
	ret.primitive = bestPrimitive;
	return ret;
}

